
#ifndef CAPTURE_CONTROL_CTRL_ZLZH_H
#define CAPTURE_CONTROL_CTRL_ZLZH_H
#include "algo_pid.h"
#include "ctrl.hpp"
#include "ctrl_conf.h"
#include "ctrl_impl.h"
#include "iir_df2tf.hpp"

class ctrl_zlzh_p0_s0 : public control::algorithm
{
private:
  algo_pid zlzh_s0_l_x{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s0_l_x"};
  algo_pid zlzh_s0_l_y{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s0_l_y"};
  algo_pid zlzh_s0_l_z{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 1, -1, "zlzh_s0_l_z"};
  algo_pid zlzh_s0_h_x{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s0_h_x"};
  algo_pid zlzh_s0_h_y{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s0_h_y"};
  algo_pid zlzh_s0_h_z{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 1, -1, "zlzh_s0_h_z"};

public:
#if 1 /*     ~~~~~~~~~~~   1hz交叉 ~~~~~~~~~~~~~~    */
    iir_df2tf<float, 4> Hpass[3] = {
        {
            {0.999892586076418, -2.964133688623638, 2.928589619018023, -0.964348516470803}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}  //
        },
        {
            {0.999892586076418, -2.964133688623638, 2.928589619018023, -0.964348516470803}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}  //
        },
        {
            {0.999892586076418, -2.964133688623638, 2.928589619018023, -0.964348516470803}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}  //
        }};

    iir_df2tf<float, 4> Lpass[3] = {
        {
            {1.0e-03 * 0.107413923582147, 1.0e-03 * 0.107844993879210, 1.0e-03 * -0.106551782988023, 1.0e-03 * -0.106982853285085}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}                                          //
        },
        {
            {1.0e-03 * 0.107413923582147, 1.0e-03 * 0.107844993879210, 1.0e-03 * -0.106551782988023, 1.0e-03 * -0.106982853285085}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}                                          //
        },
        {
            {1.0e-03 * 0.107413923582147, 1.0e-03 * 0.107844993879210, 1.0e-03 * -0.106551782988023, 1.0e-03 * -0.106982853285085}, //
            {1.000000000000000, -2.964025843629759, 2.928483067235035, -0.964455499324088}                                          //
        }};
#endif
#if 0 /*     ~~~~~~~~~~~   1.15hz交叉 ~~~~~~~~~~~~~~    */
    iir_df2tf<float, 4> Hpass[3] = {
        {
            {0.999858285028993, -2.958809262074708, 2.918043669062437, -0.959092692016722}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}  //
        },
        {
            {0.999858285028993, -2.958809262074708, 2.918043669062437, -0.959092692016722}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}  //
        },
        {
            {0.999858285028993, -2.958809262074708, 2.918043669062437, -0.959092692016722}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}  //
        }};

    iir_df2tf<float, 4> Lpass[3] = {
        {
            {1.0e-03 * 0.141714971006738, 1.0e-03 * 0.142368809361518, 1.0e-03 * -0.140407294297177, 1.0e-03 * -0.141061132651957}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}                                          //
        },
        {
            {1.0e-03 * 0.141714971006738, 1.0e-03 * 0.142368809361518, 1.0e-03 * -0.140407294297177, 1.0e-03 * -0.141061132651957}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}                                          //
        },
        {
            {1.0e-03 * 0.141714971006738, 1.0e-03 * 0.142368809361518, 1.0e-03 * -0.140407294297177, 1.0e-03 * -0.141061132651957}, //
            {1.000000000000000, -2.958666893265347, 2.917903261768140, -0.959233753149374}                                          //
        }};
#endif
#if 0 /*     ~~~~~~~~~~~   1.2hz交叉 ~~~~~~~~~~~~~~    */
    iir_df2tf<float, 4> Hpass[3] = {
        {
            {0.999845817200911, -2.957037640968297, 2.914537830333860, -0.957346006566475}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}  //
        },
        {
            {0.999845817200911, -2.957037640968297, 2.914537830333860, -0.957346006566475}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}  //
        },
        {
            {0.999845817200911, -2.957037640968297, 2.914537830333860, -0.957346006566475}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}  //
        }};

    iir_df2tf<float, 4> Lpass[3] = {
        {
            {1.0e-03 * 0.154182799089091, 1.0e-03 * 0.154925015299162, 1.0e-03 * -0.152698366668948, 1.0e-03 * -0.1534405828790}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}                                        //
        },
        {
            {1.0e-03 * 0.154182799089091, 1.0e-03 * 0.154925015299162, 1.0e-03 * -0.152698366668948, 1.0e-03 * -0.1534405828790}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}                                        //
        },
        {
            {1.0e-03 * 0.154182799089091, 1.0e-03 * 0.154925015299162, 1.0e-03 * -0.152698366668948, 1.0e-03 * -0.1534405828790}, //
            {1.000000000000000, -2.956882715952998, 2.914385131967192, -0.957499447149354}                                        //
        }};
#endif
#if 0 /*     ~~~~~~~~~~~   1.25hz交叉 ~~~~~~~~~~~~~~    */
    iir_df2tf<float, 4> Hpass[3] = {
        {
            {0.999832834383550, -2.955267610553448, 2.911036717956244, -0.955601941786347}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}  //
        },
        {
            {0.999832834383550, -2.955267610553448, 2.911036717956244, -0.955601941786347}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}  //
        },
        {
            {0.999832834383550, -2.955267610553448, 2.911036717956244, -0.955601941786347}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}  //
        }};

    iir_df2tf<float, 4> Lpass[3] = {
        {
            {1.0e-03 * 0.167165616449864, 1.0e-03 * 0.168003775961367, 1.0e-03 * -0.165489297426856, 1.0e-03 * -0.166327456938360}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}                                          //
        },
        {
            {1.0e-03 * 0.167165616449864, 1.0e-03 * 0.168003775961367, 1.0e-03 * -0.165489297426856, 1.0e-03 * -0.166327456938360}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}                                          //
        },
        {
            {1.0e-03 * 0.167165616449864, 1.0e-03 * 0.168003775961367, 1.0e-03 * -0.165489297426856, 1.0e-03 * -0.166327456938360}, //
            {1.000000000000000, -2.955099606777487, 2.910871228658818, -0.955768269243286}                                          //
        }};
#endif

#if 0 /*     ~~~~~~~~~~~   1.3hz交叉 ~~~~~~~~~~~~~~    */
  iir_df2tf<float, 4> Hpass[3] = {
      {
          {0.999819337822805, -2.953499168926411, 2.907540324384408, -0.953860493280801}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}  //
      },
      {
          {0.999819337822805, -2.953499168926411, 2.907540324384408, -0.953860493280801}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}  //
      },
      {
          {0.999819337822805, -2.953499168926411, 2.907540324384408, -0.953860493280801}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}  //
      }};

  iir_df2tf<float, 4> Lpass[3] = {
      {
          {1.0e-03 * 0.180662177194865, 1.0e-03 * 0.181604146506531, 1.0e-03 * -0.178778238571534, 1.0e-03 * -0.179720207883200}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}                                          //
      },
      {
          {1.0e-03 * 0.180662177194865, 1.0e-03 * 0.181604146506531, 1.0e-03 * -0.178778238571534, 1.0e-03 * -0.179720207883200}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}                                          //
      },
      {
          {1.0e-03 * 0.180662177194865, 1.0e-03 * 0.181604146506531, 1.0e-03 * -0.178778238571534, 1.0e-03 * -0.179720207883200}, //
          {1.000000000000000, -2.953317564779905, 2.907361546145837, -0.954040213488685}                                          //
      }};
#endif

#if 0 /*     ~~~~~~~~~~~   1.35hz交叉 ~~~~~~~~~~~~~~    */
  iir_df2tf<float, 4> Hpass[3] = {
      {
          {0.999805328761765, -2.951732314186285, 2.904048642087275, -0.952121656662755}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}  //
      },
      {
          {0.999805328761765, -2.951732314186285, 2.904048642087275, -0.952121656662755}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}  //
      },
      {
          {0.999805328761765, -2.951732314186285, 2.904048642087275, -0.952121656662755}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}  //
      }};

  iir_df2tf<float, 4> Lpass[3] = {
      {
          {1.0e-03 * 0.194671238234466, 1.0e-03 * 0.195725183792767, 1.0e-03 * -0.192563347117863, 1.0e-03 * -0.193617292676164}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}                                          //
      },
      {
          {1.0e-03 * 0.194671238234466, 1.0e-03 * 0.195725183792767, 1.0e-03 * -0.192563347117863, 1.0e-03 * -0.193617292676164}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}                                          //
      },
      {
          {1.0e-03 * 0.194671238234466, 1.0e-03 * 0.195725183792767, 1.0e-03 * -0.192563347117863, 1.0e-03 * -0.193617292676164}, //
          {1.000000000000000, -2.951536589002494, 2.903856078740159, -0.952315273955432}                                          //
      }};
#endif

#if 0 /*     ~~~~~~~~~~~   1.4hz交叉 ~~~~~~~~~~~~~~    */
  iir_df2tf<float, 4> Hpass[3] = {
      {
          {0.999790808440723, -2.949967044435011, 2.900561663547854, -0.950385427553565}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}  //
      },
      {
          {0.999790808440723, -2.949967044435011, 2.900561663547854, -0.950385427553565}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}  //
      },
      {
          {0.999790808440723, -2.949967044435011, 2.900561663547854, -0.950385427553565}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}  //
      }};

  iir_df2tf<float, 4> Lpass[3] = {
      {
          {1.0e-03 * 0.209191559276876, 1.0e-03 * 0.210365946374513, 1.0e-03 * -0.206842785081601, 1.0e-03 * -0.208017172179238}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}                                          //
      },
      {
          {1.0e-03 * 0.209191559276876, 1.0e-03 * 0.210365946374513, 1.0e-03 * -0.206842785081601, 1.0e-03 * -0.208017172179238}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}                                          //
      },
      {
          {1.0e-03 * 0.209191559276876, 1.0e-03 * 0.210365946374513, 1.0e-03 * -0.206842785081601, 1.0e-03 * -0.208017172179238}, //
          {1.000000000000000, -2.949756678488638, 2.900354820762773, -0.950593444725745}                                          //
      }};
#endif

#if 0 /*     ~~~~~~~~~~~   1.45hz交叉 ~~~~~~~~~~~~~~    */
  iir_df2tf<float, 4> Hpass[3] = {
      {
          {0.999775778097179, -2.948203357777364, 2.897079381263192, -0.948651801583006}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}  //
      },
      {
          {0.999775778097179, -2.948203357777364, 2.897079381263192, -0.948651801583006}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}  //
      },
      {
          {0.999775778097179, -2.948203357777364, 2.897079381263192, -0.948651801583006}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}  //
      }};

  iir_df2tf<float, 4> Lpass[3] = {
      {
          {1.0e-03 * 0.224221902821446, 1.0e-03 * 0.225525494499149, 1.0e-03 * -0.221614719466040, 1.0e-03 * -0.222918311143743}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}                                          //
      },
      {
          {1.0e-03 * 0.224221902821446, 1.0e-03 * 0.225525494499149, 1.0e-03 * -0.221614719466040, 1.0e-03 * -0.222918311143743}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}                                          //
      },
      {
          {1.0e-03 * 0.224221902821446, 1.0e-03 * 0.225525494499149, 1.0e-03 * -0.221614719466040, 1.0e-03 * -0.222918311143743}, //
          {1.000000000000000, -2.947977832282865, 2.896857766543727, -0.948874719894151}                                          //
      }};
#endif

public:
  void Process() override // 控制程序
  {
    // 计算位移信号PID 低通滤波
    zlzh_s0_l_x.fb = Lpass[0].update(ctrl.impl->sensor_cd22_xyz[0]);
    zlzh_s0_l_y.fb = Lpass[1].update(ctrl.impl->sensor_cd22_xyz[1]);
    zlzh_s0_l_z.fb = Lpass[2].update(ctrl.impl->sensor_cd22_xyz[2]);

    zlzh_s0_l_x.update();
    zlzh_s0_l_y.update();
    zlzh_s0_l_z.update();

    // 计算速度信号PID高通滤波
    zlzh_s0_h_x.fb = Hpass[0].update(ctrl.impl->sensor_941b_xyz[0]);
    zlzh_s0_h_y.fb = Hpass[1].update(ctrl.impl->sensor_941b_xyz[1]);
    zlzh_s0_h_z.fb = Hpass[2].update(ctrl.impl->sensor_941b_xyz[2]);

    zlzh_s0_h_x.update();
    zlzh_s0_h_y.update();
    zlzh_s0_h_z.update();

    ctrl.impl->ta115_LAC08_xyz[0] = zlzh_s0_l_x.u + zlzh_s0_h_x.u;
    ctrl.impl->ta115_LAC08_xyz[1] = zlzh_s0_l_y.u + zlzh_s0_h_y.u;
    ctrl.impl->ta115_LAC08_xyz[2] = zlzh_s0_l_z.u + zlzh_s0_h_z.u;

    // 转换到 原始坐标
    ctrl.impl->ta115_LAC08 = ctrl.impl->Ta_mat * ctrl.impl->ta115_LAC08_xyz;
  }
  void Init() override // 初始化等
  {
    par_agent::get_instance().reg(zlzh_s0_l_x);
    par_agent::get_instance().reg(zlzh_s0_l_y);
    par_agent::get_instance().reg(zlzh_s0_l_z);
    par_agent::get_instance().reg(zlzh_s0_h_x);
    par_agent::get_instance().reg(zlzh_s0_h_y);
    par_agent::get_instance().reg(zlzh_s0_h_z);
  }
};

class ctrl_zlzh_p0_s1 : public ctrl_zlzh_p0_s0
{
public:
  algo_pid zlzh_s1_l_x{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s1_l_x"};
  algo_pid zlzh_s1_l_y{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s1_l_y"};
  algo_pid zlzh_s1_l_z{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 1, -1, "zlzh_s1_l_z"};
  algo_pid zlzh_s1_h_x{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s1_h_x"};
  algo_pid zlzh_s1_h_y{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 2, -2, "zlzh_s1_h_y"};
  algo_pid zlzh_s1_h_z{(float) CTRL_OVER_SAMPLE / (float) CTRL_FREQ, 1, -1, "zlzh_s1_h_z"};

  iir_df2tf<float, 3> OutHpass[3] = {
      {
          {0.998794356086686, -1.997588712173372, 0.998794356086686}, //
          {1.000000000000000, -1.997587258595598, 0.997590165751146}  //
      },
      {
          {0.998794356086686, -1.997588712173372, 0.998794356086686}, //
          {1.000000000000000, -1.997587258595598, 0.997590165751146}  //
      },
      {
          {0.998794356086686, -1.997588712173372, 0.998794356086686}, //
          {1.000000000000000, -1.997587258595598, 0.997590165751146}  //
      }};

  iir_df2tf<float, 3> OutLpass[3] = {
      {
          {0.022911747394679, 0.045823494789358, 0.022911747394679}, //
          {1.000000000000000, -1.528507400761278, 0.620154390339995} //
      },
      {
          {0.022911747394679, 0.045823494789358, 0.022911747394679}, //
          {1.000000000000000, -1.528507400761278, 0.620154390339995} //
      },
      {
          {0.022911747394679, 0.045823494789358, 0.022911747394679}, //
          {1.000000000000000, -1.528507400761278, 0.620154390339995} //
      }};

public:
  void Process() override // 控制程序
  {
    // 计算位移信号PID 低通滤波
    zlzh_s1_l_x.fb = Lpass[0].update(ctrl.impl->sensor_cd22_xyz[0]);
    zlzh_s1_l_y.fb = Lpass[1].update(ctrl.impl->sensor_cd22_xyz[1]);
    zlzh_s1_l_z.fb = Lpass[2].update(ctrl.impl->sensor_cd22_xyz[2]);

    zlzh_s1_l_x.update();
    zlzh_s1_l_y.update();
    zlzh_s1_l_z.update();

    // 计算速度信号PID高通滤波
    zlzh_s1_h_x.fb = Hpass[0].update(ctrl.impl->sensor_941b_xyz[0]);
    zlzh_s1_h_y.fb = Hpass[1].update(ctrl.impl->sensor_941b_xyz[1]);
    zlzh_s1_h_z.fb = Hpass[2].update(ctrl.impl->sensor_941b_xyz[2]);

    zlzh_s1_h_x.update();
    zlzh_s1_h_y.update();
    zlzh_s1_h_z.update();

    ctrl.impl->ta115_LAC08_xyz[0] = OutHpass[0].update(OutLpass[0].update(zlzh_s1_l_x.u + zlzh_s1_h_x.u));
    ctrl.impl->ta115_LAC08_xyz[1] = OutHpass[1].update(OutLpass[1].update(zlzh_s1_l_y.u + zlzh_s1_h_y.u));
    ctrl.impl->ta115_LAC08_xyz[2] = OutHpass[2].update(OutLpass[2].update(zlzh_s1_l_z.u + zlzh_s1_h_z.u));

    // 转换到 原始坐标
    ctrl.impl->ta115_LAC08 = ctrl.impl->Ta_mat * ctrl.impl->ta115_LAC08_xyz;
  }

  void Init() override // 初始化等
  {
    par_agent::get_instance().reg(zlzh_s1_l_x);
    par_agent::get_instance().reg(zlzh_s1_l_y);
    par_agent::get_instance().reg(zlzh_s1_l_z);
    par_agent::get_instance().reg(zlzh_s1_h_x);
    par_agent::get_instance().reg(zlzh_s1_h_y);
    par_agent::get_instance().reg(zlzh_s1_h_z);
  }
};
#endif // CAPTURE_CONTROL_CTRL_ZLZH_H
